#include "PathDataParser.h"
#include <QDebug>
#include <Common/SysEnv.h>

PathDataParser::PathDataParser(QObject *parent)
    : QObject(parent)
    , m_totalPoints(0)
    , m_totalCommands(0)
{
}

// 在 parsePathDataGrouped 函数中添加最后一个路径点的Z轴修改
QVector<QString> PathDataParser::parsePathDataGrouped(const s_LzppRtsPara& pathResult)
{
    QVector<QString> commands;
    m_totalPoints = 0;
    m_totalCommands = 0;

    std::vector<s_PPts> pathPoints = pathResult.sListPPtsRobot; // 改为非const，以便修改

    if (pathPoints.empty()) {
        return commands;
    }

    // 修改最后一个路径点的Z轴为21.5
    if (!pathPoints.empty()) {
        pathPoints.back().fZ = 21.5;
        qDebug() << "修改最后一个路径点Z轴为21.5";
    }

    // 1. 创建分组
    QVector<QPair<int, int>> groups;
    QVector<QSet<int>> groupCylinders;
    createPathPointGroups(pathPoints, groups, groupCylinders);

    // 2. 合并无气缸分组
    QVector<QPair<int, int>> finalGroups;
    QVector<QSet<int>> finalCylinders;
    mergeEmptyCylinderGroups(groups, groupCylinders, finalGroups, finalCylinders);

    // 3. 调试输出分组信息
    debugOutputGroups(finalGroups, finalCylinders, pathPoints);

    // 4. 生成指令
    QSet<int> previousCylinders;

    for (int groupIdx = 0; groupIdx < finalGroups.size(); groupIdx++) {
        int startIdx = finalGroups[groupIdx].first;
        int endIdx = finalGroups[groupIdx].second;
        QSet<int> currentCylinders = finalCylinders[groupIdx];
        double originalZHeight = pathPoints[startIdx].fZ;
        int originalCNType = (pathPoints[startIdx].iDef == 4) ? 210 : 211;

        // 处理分组
        if (groupIdx == 0) {
            processFirstGroup(commands, currentCylinders, originalZHeight, pathPoints[startIdx]);
        } else {
            processGroupTransition(commands, previousCylinders, currentCylinders, originalZHeight, originalCNType);
        }

        // 处理组内移动
        processGroupMovement(commands, finalGroups[groupIdx], pathPoints);

        previousCylinders = currentCylinders;
    }

    // 5. 最终处理
    processFinalCommands(commands, previousCylinders);

    qDebug() << QString("=== 解析完成: %1点 -> %2指令 ===").arg(m_totalPoints).arg(m_totalCommands);

    return commands;
}

// 分组相关函数
void PathDataParser::createPathPointGroups(const std::vector<s_PPts>& pathPoints,
                                           QVector<QPair<int, int>>& groups,
                                           QVector<QSet<int>>& groupCylinders)
{
    int currentStart = 0;
    QSet<int> currentCylinders = parseCylinderIndexes(pathPoints[0].strQgNotSafe);

    for (size_t i = 1; i < pathPoints.size(); i++) {
        QSet<int> nextCylinders = parseCylinderIndexes(pathPoints[i].strQgNotSafe);

        bool shouldSplit = false;
        bool currentHasCylinders = !currentCylinders.isEmpty();
        bool nextHasCylinders = !nextCylinders.isEmpty();

        if (nextHasCylinders) {
            if (currentHasCylinders) {
                // 连续气缸合并
                currentCylinders.unite(nextCylinders);
            } else {
                // 相同气缸合并检查
                bool hasPreviousGroup = !groups.isEmpty();
                if (hasPreviousGroup) {
                    QSet<int> lastGroupCylinders = groupCylinders.last();
                    if (lastGroupCylinders == nextCylinders) {
                        groups.last().second = i;
                        groupCylinders.last() = nextCylinders;
                        currentStart = i + 1;
                        currentCylinders = QSet<int>();
                        continue;
                    } else {
                        shouldSplit = true;
                    }
                } else {
                    shouldSplit = true;
                }
            }
        } else {
            if (currentHasCylinders) {
                shouldSplit = true;
            }
        }

        if (shouldSplit) {
            if (currentStart <= static_cast<int>(i-1)) {
                groups.append(QPair<int, int>(currentStart, i-1));
                groupCylinders.append(currentCylinders);
            }
            currentStart = i;
            currentCylinders = nextCylinders;
        }
    }

    if (currentStart < static_cast<int>(pathPoints.size())) {
        groups.append(QPair<int, int>(currentStart, pathPoints.size()-1));
        groupCylinders.append(currentCylinders);
    }
}

void PathDataParser::mergeEmptyCylinderGroups(const QVector<QPair<int, int>>& groups,
                                              const QVector<QSet<int>>& groupCylinders,
                                              QVector<QPair<int, int>>& finalGroups,
                                              QVector<QSet<int>>& finalCylinders)
{
    for (int i = 0; i < groups.size(); i++) {
        if (finalGroups.isEmpty()) {
            finalGroups.append(groups[i]);
            finalCylinders.append(groupCylinders[i]);
            continue;
        }

        if (groupCylinders[i].isEmpty() && !finalCylinders.last().isEmpty()) {
            finalGroups.last().second = groups[i].second;
            qDebug() << QString("无气缸分组合并: 组%1[%2-%3]合并到前组")
                            .arg(i).arg(groups[i].first).arg(groups[i].second);
        } else {
            finalGroups.append(groups[i]);
            finalCylinders.append(groupCylinders[i]);
        }
    }
}

// 指令生成相关函数
void PathDataParser::processFirstGroup(QVector<QString>& commands,
                                       const QSet<int>& currentCylinders,
                                       double originalZHeight,
                                       const s_PPts& firstPoint)
{
    if (!currentCylinders.isEmpty()) {
        QString liftZ = generateLiftZCommand(21.5);
        commands.append(liftZ);
        m_totalCommands++;
        qDebug() << "第一组抬升Z轴到安全高度:" << liftZ;

        QString lift = generateLiftCylinderCommand(currentCylinders);
        commands.append(lift);
        m_totalCommands++;

        QStringList currCylList;
        for (int index : currentCylinders) {
            currCylList.append(QString::number(index));
        }
        qDebug() << QString("抬起第一组气缸[%1]: %2").arg(currCylList.join(",")).arg(lift);

        QString firstMoveCmd = generateMoveCommand(firstPoint);
        commands.append(firstMoveCmd);
        m_totalCommands++;
        m_totalPoints++;
        qDebug() << QString("第一组先移动到起点: %1").arg(firstMoveCmd);
    }
}

void PathDataParser::processGroupTransition(QVector<QString>& commands,
                                            const QSet<int>& previousCylinders,
                                            const QSet<int>& currentCylinders,
                                            double originalZHeight,
                                            int originalCNType)
{
    if (previousCylinders != currentCylinders) {
        // 1. 抬升Z轴到安全高度
        QString liftZ = generateLiftZCommand(21.5);
        commands.append(liftZ);
        m_totalCommands++;
        qDebug() << "抬升Z轴到安全高度:" << liftZ;

        // 2. 放下前一组的气缸
        if (!previousCylinders.isEmpty()) {
            QString putDown = generatePutDownCylinderCommand(previousCylinders);
            commands.append(putDown);
            m_totalCommands++;

            QStringList prevCylList;
            for (int index : previousCylinders) {
                prevCylList.append(QString::number(index));
            }
            qDebug() << QString("放下前一组气缸[%1]: %2").arg(prevCylList.join(",")).arg(putDown);
        }

        // 3. 抬起当前组的气缸
        if (!currentCylinders.isEmpty()) {
            QString lift = generateLiftCylinderCommand(currentCylinders);
            commands.append(lift);
            m_totalCommands++;

            QStringList currCylList;
            for (int index : currentCylinders) {
                currCylList.append(QString::number(index));
            }
            qDebug() << QString("抬起当前组气缸[%1]: %2").arg(currCylList.join(",")).arg(lift);
        }

        // 4. 分段放下Z轴
        addSegmentedLowerZCommands(commands, originalZHeight, originalCNType);
    }
}

// 在 processGroupMovement 函数中添加高度检查
void PathDataParser::processGroupMovement(QVector<QString>& commands,
                                          const QPair<int, int>& group,
                                          const std::vector<s_PPts>& pathPoints)
{
    for (int i = group.first; i <= group.second; i++) {
        // 检查是否需要抬升Z轴（从第二个点开始检查）
        if (i > group.first) {
            double currentZ = pathPoints[i-1].fZ;
            double nextZ = pathPoints[i].fZ;
            int nextCNType = (pathPoints[i].iDef == 4) ? 210 : 211;

            addLiftZIfNeeded(commands, currentZ, nextZ, nextCNType);
        }

        QString moveCmd = generateMoveCommand(pathPoints[i]);
        commands.append(moveCmd);
        m_totalCommands++;
        m_totalPoints++;

        if (i == group.first || i == group.second || i % 10 == 0) {
            qDebug() << QString("移动点%1: %2").arg(i).arg(moveCmd);
        }
    }
}

void PathDataParser::processFinalCommands(QVector<QString>& commands, const QSet<int>& lastCylinders)
{
    QString finalLiftZ = generateLiftZCommand(21.5);
    commands.append(finalLiftZ);
    m_totalCommands++;
    qDebug() << "最终抬升Z轴:" << finalLiftZ;

    QString finalGoBack = generateLiftGoBackCommand();
    commands.append(finalGoBack);
    m_totalCommands++;
    qDebug() << "回归远点:" << finalGoBack;

    if (!lastCylinders.isEmpty()) {
        QString finalPutDown = generatePutDownCylinderCommand(lastCylinders);
        commands.append(finalPutDown);
        m_totalCommands++;

        QStringList prevCylList;
        for (int index : lastCylinders) {
            prevCylList.append(QString::number(index));
        }
        qDebug() << QString("最终放下气缸[%1]: %2").arg(prevCylList.join(",")).arg(finalPutDown);
    }
}

// 分段下降函数
void PathDataParser::addSegmentedLowerZCommands(QVector<QString>& commands, double targetZHeight, int originalCNType)
{
    if (originalCNType == 211) {
        // 切割指令211，需要分段下降
        double intermediateZ = targetZHeight + 10.0;
        QString intermediateCmd = QString("CN=210;Flag=0;CP=&&EZ=%1&&")
                                      .arg(intermediateZ, 0, 'f', 1);
        commands.append(intermediateCmd);
        m_totalCommands++;
        qDebug() << QString("分段下降第一步(空泡210): 到Z=%1").arg(intermediateZ);

        // 第二步：切割下降到目标高度
        SysEnv& env = SysEnv::instance();
        const ProcessParams& processPara = env.cfg().processParams;
        QString finalCmd = QString("CN=211;Flag=0;CP=&&EZ=%1,V=%2,R=%3&&")
                               .arg(targetZHeight, 0, 'f', 1)
                               .arg(processPara.feedRate, 0, 'f', 1)
                               .arg(processPara.toolSpeed, 0, 'f', 1);
        commands.append(finalCmd);
        m_totalCommands++;
        qDebug() << QString("分段下降第二步(切割211): 到Z=%1").arg(targetZHeight);
    } else {
        // 空泡指令210，直接下降
        QString lowerZ = generateLiftZCommand(targetZHeight);
        commands.append(lowerZ);
        m_totalCommands++;
        qDebug() << "直接下降(空泡210):" << lowerZ;
    }
}

// 工具函数
void PathDataParser::debugOutputGroups(const QVector<QPair<int, int>>& groups,
                                       const QVector<QSet<int>>& groupCylinders,
                                       const std::vector<s_PPts>& pathPoints)
{
    qDebug() << "=== 最终分组信息（连续气缸+相同气缸合并） ===";
    for (int i = 0; i < groups.size(); i++) {
        int start = groups[i].first;
        int end = groups[i].second;
        QSet<int> cyl = groupCylinders[i];

        QStringList cylList;
        for (int index : cyl) {
            cylList.append(QString::number(index));
        }

        qDebug() << QString("分组%1: 点%2-%3, 气缸: [%4]")
                        .arg(i).arg(start).arg(end)
                        .arg(cylList.isEmpty() ? "无" : cylList.join(","));

        for (int j = start; j <= end; j++) {
            QSet<int> pointCyl = parseCylinderIndexes(pathPoints[j].strQgNotSafe);
            QStringList pointCylList;
            for (int index : pointCyl) {
                pointCylList.append(QString::number(index));
            }
            qDebug() << QString("  点%1: 气缸[%2]")
                            .arg(j)
                            .arg(pointCylList.isEmpty() ? "无" : pointCylList.join(","));
        }
    }
}

// 新增函数：检查并添加Z轴抬升指令
void PathDataParser::addLiftZIfNeeded(QVector<QString>& commands, double currentZ, double nextZ, int nextCNType)
{
    // 只有210（空泡）指令需要检查高度抬升，211（切割）不需要
    if (nextCNType == 210 && nextZ > currentZ) {
        QString liftZ = generateLiftZCommand(nextZ);
        commands.append(liftZ);
        m_totalCommands++;

        qDebug() << QString("高度检测(空泡210): %1 -> %2，抬升Z轴到 %3")
                        .arg(currentZ, 0, 'f', 1)
                        .arg(nextZ, 0, 'f', 1)
                        .arg(nextZ, 0, 'f', 1);
    }
}

// 以下为基础指令生成函数（保持不变）
QString PathDataParser::generateMoveCommand(const s_PPts& point)
{
    int cnType = (point.iDef == 4) ? 210 : 211;
    SysEnv& env = SysEnv::instance();
    const ProcessParams& processPara = env.cfg().processParams;
    QString command;
    if (cnType == 211) {
        command = QString("CN=%1;Flag=0;CP=&&EX=%2,EY=%3,EZ=%4,V=%5,R=%6&&")
        .arg(cnType)
            .arg(point.fX, 0, 'f', 1)
            .arg(point.fY, 0, 'f', 1)
            .arg(point.fZ, 0, 'f', 1)
            .arg(processPara.feedRate, 0, 'f', 1)
            .arg(processPara.toolSpeed, 0, 'f', 1);
    } else {
        command = QString("CN=%1;Flag=0;CP=&&EX=%2,EY=%3,EZ=%4&&")
        .arg(cnType)
            .arg(point.fX, 0, 'f', 1)
            .arg(point.fY, 0, 'f', 1)
            .arg(point.fZ, 0, 'f', 1);
    }
    return command;
}

QString PathDataParser::generateLiftCylinderCommand(const QSet<int>& cylinderIndexes)
{
    QString hexBits = cylinderIndexesToHex(cylinderIndexes);
    return QString("CN=304;Flag=0;CP=&&IOBits=%1&&").arg(hexBits);
}

QString PathDataParser::generatePutDownCylinderCommand(const QSet<int>& cylinderIndexes)
{
    QString hexBits = cylinderIndexesToHex(cylinderIndexes);
    return QString("CN=303;Flag=0;CP=&&IOBits=%1&&").arg(hexBits);
}

QString PathDataParser::generateLiftZCommand(double zHeight)
{
    return QString("CN=210;Flag=0;CP=&&EZ=%1&&").arg(zHeight, 0, 'f', 1);
}

QString PathDataParser::generateLiftGoBackCommand()
{
    return QString("CN=210;Flag=0;CP=&&EX=0,EY=0,EZ=21.5&&");
}

QSet<int> PathDataParser::parseCylinderIndexes(const std::string& cylinderStr)
{
    QSet<int> result;
    QString cleanedStr = QString::fromStdString(cylinderStr);
    cleanedStr = cleanedStr.replace("[", "").replace("]", "").trimmed();

    if (cleanedStr.isEmpty()) {
        return result;
    }

    QStringList indexList = cleanedStr.split(',', Qt::SkipEmptyParts);
    for (const QString& indexStr : indexList) {
        bool ok;
        int index = indexStr.trimmed().toInt(&ok);
        if (ok && index >= 0 && index <= 15) {
            result.insert(index);
        }
    }
    return result;
}

QString PathDataParser::cylinderIndexesToHex(const QSet<int>& cylinderIndexes)
{
    if (cylinderIndexes.isEmpty()) {
        return "00000";
    }

    int binaryValue = 0;
    for (int index : cylinderIndexes) {
        if (index >= 0 && index <= 15) {
            binaryValue |= (1 << index);
        }
    }
    return QString("%1").arg(binaryValue, 5, 16, QChar('0')).toUpper();
}

void PathDataParser::getParseStats(int &totalPoints, int &totalCommands) const
{
    totalPoints = m_totalPoints;
    totalCommands = m_totalCommands;
}
